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1.  Introduction 


The  find-path  problem  is  well  known  in  robotics.  Given  an  object  with  an  initial 
location  and  orientation,  a  goal  location  and  orientation,  and  a  set  of  obstacles  located  in 
space,  the  problem  is  to  find  a  continuous  path  for  the  object  from  the  initial  position  to 
the  goal  position  which  avoids  collisions  with  obstacles  along  the  way.  This  paper  presents 
a  new  representation  for  free  space,  as  natural  “freeways”  between  obstacles,  and  a  new 
algorithm  to  solve  find-path  using  that  representation.  The  algorithm's  advantages  over 
those  previously  presented  are  that  it  is  quite  fast  and  it  finds  good  paths  which  generously 
avoid  obstacles  rather  than  barely  avoiding  them.  It  does  not  find  possible  paths  in  ex¬ 
tremely  cluttered  situations  but  it  can  be  used  to  provide  direction  to  more  computationally 
expensive  algorithms  in  those  cases. 

1.1  Approaches  to  the  Find-Path  problem. 

A  common  approach  to  the  find-path  problem,  used  with  varying  degrees  of  sophistica¬ 
tion,  is  the  configuration  space  approach.  Lozano-P6rez  [5]  gives  a  thorough  mathematical 
treatment  of  configuration  space.  The  idea  is  to  determine  those  parts  of  free  space  which 
a  reference  point  of  the  moving  object  can  occupy  without  colliding  with  any  obstacles. 
A  path  is  then  found  for  the  reference  point  through  this  truly  free  space.  Dealing  with 
rotations  turns  out  to  be  a  major  difficulty  with  the  approach  -  requiring  complex  geometric 
algorithms  which  are  computationally  expensive. 

Conceptually  one  can  view  configuration  space  as  shrinking  the  object  to  a  point  while 
at  the  same  time  expanding  the  obstacles  inversely  to  the  shape  of  the  moving  object.  This 
approach  works  well  when  the  moving  object  is  not  allowed  to  rotate.  If  it  can  rotate  then 
the  grown  obstacles  must  be  embedded  in  a  higher  dimensional  space  -  one  extra  dimen¬ 
sion  for  each  degree  of  rotational  freedom.  Furthermore  the  grown  obstacles  have  non- 
planar  surfaces,  even  when  the  original  problem  was  completely  polygonal  or  polyhedral. 
Typically  implementors  have  approximated  the  grown  obstacles  in  order  to  be  able  to  deal 
with  rotations. 
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Moravec  [6]  had  to  solve  the  find-path  problem  in  two  dimensions,  lie  bounded  all 
obstacles  and  the  moving  object  by  circles.  Then  the  grown  obstacles  were  all  perpendicular 
cylinders  and  the  problem  could  be  projected  back  into  two  dimensions  where  rotations 
could  be  ignored.  This  method  missed  all  paths  that  required  rotational  manouevering. 

Lozano-Ptrez  [5j  split  the  rotation  range  into  a  fixed  number  of  slices,  and  within  each 
slice  bounded  the  grown  obstacles  by  polyhedra.  Earlier  Udupa  [8]  had  used  much  cruder 
bounding  polyhedra  in  a  similar  way. 

Recently  Brooks  and  Lozano-Perez  [3]  developed  a  method  for  computing  more  directly 
with  the  curved  surfaces  of  the  grown  obstacles.  The  algorithm  can  be  quite  expensive 
although  given  enough  time  and  space  it  can  find  a  path  if  one  exists. 

1.2  Good  representations  simplify  Find-Path. 

The  algorithm  presented  in  this  paper  is  based  on  a  different  idea;  that  of  using  rep¬ 
resentations  of  space  which  capture  the  essential  effects  of  translating  and  rotating  a  body 
through  space.  Free  space  is  represented  as  overlapping  generalized  cones  since  they  are 
descriptions  of  swept  volumes.  The  cones  provide  a  high  level  plan  for  moving  the  object 
through  space.  The  volume  swept  by  the  particular  object  as  it  is  translated  and  rotated 
is  characterized  as  a  function  of  its  orientation.  Find-path  then  reduces  to  comparing  the 
swept  volume  of  the  object  with  the  sweepable  volumes  of  free  space.  This  is  done  by 
inverting  the  characterization  of  the  volume  swept  by  an  object,  to  determine  its  valid 
orientations  as  it  moves  through  a  generalized  cone. 


Throughout  this  paper  we  restrict  our  attention  to  the  two-dimensional  problem  where 
the  object  to  be  moved  is  a  convex  polygon  and  the  obstacles  are  represented  as  unions  of 
convex  polygons. 
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2.  Describing  Free  Space  as  Generalized  Cones 

Generalized  cones  are  a  commonly  used  representation  of  volume  for  modelling  ob¬ 
jects  in  artificial  intelligence  based  computer  vision  systems.  They  were  first  introduced  by 
Binford  [2].  A  generalized  cone  is  formed  by  sweeping  a  two  dimensional  cross  section  along 
a  curve  in  space,  called  a  spine,  and  deforming  it  according  to  a  sweeping  rule . 

In  this  paper  we  will  consider  a  two  dimensional  specialization  of  generalized  cones 
(although  we  will  still  refer  to  them  as  volumes).  The  spines  will  be  straight  and  the  cross 
sections  will  be  line  segments  held  perpendicular  to  the  spine  with  left  and  right  radii. 
The  sweeping  rule  will  independently  control  the  magnitudes  of  the  left  and  right  radii  as 
piecewise  linear  functions. 

Free  space  is  represented  as  overlapping  generalized  cones.  Natural  “freeways”,  elon¬ 
gated  regions  through  which  the  object  might  be  moved,  arc  represented  as  generalized 
cones.  The  generalized  cones  overlap  at  intersections  of  these  natural  freeways.  Figure  1 
illustrates  a  few  of  the  generalized  cones  describing  the  free  space  around  two  obstacles  in 
a  confined  workspace. 

2.1  Candidate  generalized  cones. 

A  representation  of  free  space  is  constructed  by  examining  all  pairs  of  edges  of  obstacle 
polygons.  If  the  edges  define  a  natural  “freeway”  through  space  they  are  used  to  construct 
a  generalized  cone. 

Each  edge  has  a  “free”  side  and  a  “full”  side,  where  the  “free”  side  is  the  outside  of  the 
particular  polygon  with. which  it  is  associated.  Each  edge  has  an  outward  pointing  normal, 

*  *  ■'  i 

pointing  into  the  “free"  side*.  Two  edges  are  accepted  as  defining  a  candidate  generalized 
coneif  they  meet  the  following  two  requirements.  (1.)  At  least  one  vertex  of  each  edge 
should  be  on  the  “free”  side  of  the  other.  (2.)  The  dot  product  of  the  outward  pointing 
normal's  should  be  negative.  The  second  condition  ensures  that  the  “free”  sides  of  the  two 
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Figure  1.  A  few  of  the  generalized  cones  generated  by  two  obstacles  and  the  workspace 
boundary. 

edges  essentially  face  each  other.  Thus  in  this  initial  stage  there  is  a  complexity  factor  of 
0(n2),  where  n  is  the  number  of  edges  of  obstacle  polygons. 

Given  a  candidate  pair  of  edges,  a  spine  is  constructed  for  the  generalized  cone.  It  is  the 
bisector  of  the  space  which  is  on  the  “free”  side  of  both  edges.  (Thus  if  the  edges  are  parallel 
the  spine  is  parallel  to  them  both,  and  equidistant,  or  if  the  edges  are  not  parallel  then  the 
spine  bisects  the  angle  they  form.)  The  generalized  cone  occupies  the  volume  between  the 
two  defining  edges.  At  each  vertex  of  the  two  edges  (if  the  vertex  is  on  the  "free”  side  of 
the  other  edge)  the  cone  is  extended  parallel  to  the  spine. 

The  generalized  cone  so  defined  may  not  lie  entirely  in  free  space.  There  may  be  other 
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Figure  2.  Slices  of  the  generalized  cone  removed  due  to  obstacles. 

obstacles  which  intersect  it.  Each  obstacle  is  compared  to  the  generalized  cone.  A  polygon 
can  be  intersected  with  the  cone  in  time  O(n)  where  n  is  the  number  of  edges  of  the  polygon. 
If  the  intersection  is  empty  then  nothing  further  need  be  done.  If  not  then  the  intersection 
is  projected  normally  onto  the  the  spine  of  the  generalized  cone.  This  is  illustrated  in  figure 
2.  Again  this  is  an  O(n)  operation  in  the  number  of  vertices  (and  hence  edges).  The  result  of 
comparing  all  obstacles  to  the  generalized  cone  is  a  set  of  regions  of  the  spine  where  there  is 
no  obstacle  which  intersects  the  cone  in  a  slice  normal  to  the  spine.  Each  disjoint  slice  which 
includes  parts  of  the  original  two  edges  is  then  accepted  as  a  generalized  cone  describing 
part  of  free  space. 

Clearly  the  complete  operation  of  describing  free  space  is  at  most  0(n3)  in  the  number 
of  edges  in  the  obstacle  polygons. 
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Figure  3.  A  generalized  cone  defined  by  bit  br ,  srt  m  and  c. 

2.2  The  representation  used  for  generalized  cones. 

Figure  3  shows  the  complete  representation  used  for  cones  describing  parts  of  free  space. 
There  is  a  straight  spine,  parameterized  over  the  range  t  £  [0,  /]  where  l  is  the  length  of  the 
cone.  If  the  sides  of  the  cone  are  not  parallel  to  the  spine  then  t  =  0  corresponds  to  the 
wider  end. 


On  both  the  left  and  right  the  maximal  radii  achieved  over  the  length  of  the  cone  occurs 
at  t  =  0  and  arc  denoted  6j  and  br  respectively,  describing  the  "big”  end  of  the  cone.  The 
minimal  radii  achieved  occur  at  t  =  l  and  are  denoted  st  and  sr,  describing  the  "small”  end 
of  the  cone. 

If  bi  =  *  and  br  =  sr  then  the  two  sides  of  the  cone  are  parallel  to  the  spine.  If  not,  then 
there  is  a  symmetric  thinning  of  the  cone  (which  may  start,  and  end,  at  different  values  of 
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t  on  the  left  and  right)  where  the  left  and  right  radii  of  the  thinning  parts  of  the  cone  are 
both  given  by  the  expression  mt  -f-  c  where  m  and  c  are  constants.  Note  that  it  is  always 
the  case  that  m  <  0  and  c  >  0. 

In  summary,  the  seven  constants  l,  6/,  br,  8t,  sn  m  and  c  completely  specify  the  shape 
and  size  of  the  generalized  cone.  In  addition  its  location  and  orientation  must  be  determined 
concurrently  with  computing  these  parameters. 

3.  Determining  Legal  Orientations 

Let  the  moving  object  be  called  A.  It  is  a  convex  polygon  and  has  vertices  0|,  02,  . . . , 
On.  Choose  an  origin  and  x  and  y  axes.  Let  dt  be  the  distance  of  vertex  a*  from  the  origin. 
For  optimal  performance  of  the  find-path  algorithm  the  origin  should  be  chosen  so  that 
*8  minimized  over  possible  origins.  The  direction  of  the  x-axis  can  be  chosen 

arbitrarily. 

Orientations  relative  to  A  are  defined  in  terms  of  an  angle  relative  to  A*s  x-axis.  Thus 
a  point’s  angle  relative  to  A  is  the  angle  made  by  a  ray  from  the  origin  to  the  point,  relative 
to  the  x-axis.  A  direction  relative  to  A  is  the  angle  made  by  a  vector  in  that  direction  with 
A* s  x-axis. 

Let  *7i,  Jfa,  •  •  •  1  Vn  be  the  angles  of  the  vertices  aj,  02,  . . . ,  On. 

3.1  The  radius  function  of  a  polygon. 

Consider  first  the  problem  of  determining  the  volume  swept  by  a  polygon  as  it  is  moved 
in  a  straight  line  with  fixed  orientation.  Let  the  object  be  moving  in  direction  0.  The  swept 
volume  depends  on  the  object’s  cross  section  perpendicular  to  that  angle.  The  cross  section 
can  be  broken  into  two  components;  that  on  the  left  of  the  direction  of  movement,  and  that 
on  the  right.  Figure  4  illustrates. 
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Figure  4.  Volume  swept  by  an  object  during  pure  translation. 

Define  a  radius  function  R(£)  to  be  the  infimum  of  the  distance  from  the  origin  that  a 
line  which  is  normal  to  a  ray  at  angle  £  can  be  without  intersecting  the  interior  of  A.  Figure 
5  illustrates.  (The  radius  function  is  closely  related  to  the  support  of  a  convex  polygon  - 
e.g.  see  [1].)  The  magnitudes  of  the  left  and  right  cross  sections  of  figure  4  can  then  be 
denoted  R(0  -(-  tt/2)  and  R{6  —  tt/2)  respectively. 

Figure  6  shows  the  geometric  construction  of  R{£)  for  a  given  object  A  and  angle 
The  darkened  outline  in  figure  7  shows  the  radius  function  R  in  polar  coordinates  for  the 
same  object.  Thus  R  can  be  defined  by 

R{£)  =  max  <fjCos(£  —  rji). 

l<i<n 

The  major  interest  in  functions  of  this  form  will  be  in  their  inverse  images  of  intervals 
of  the  form  ( — oo,  r].  Let 

R-4r)  =  U|R(£)<r}. 

Thus  R-L(r)  is  the  range  of  angles  at  which  the  cross  section  of  object  A  is  no  more  than  r. 
The  inverse  image  can  be  easily  computed  by  using  the  two  values  possible  for  arccos (r/d,) 
for  each  i  to  form  an  interval  containing  ^  and  subtracting  it  from  the  interval  [0, 2x]. 

Wc  restrict  our  attention  to  moving  objects  which  are  convex  polygons  precisely  because 
their  radius  functions  can  be  so  easily  inverted. 


If  we  write  Ru[£)  =  R{£  +  a)  then  the  legal  orientations  of  A  relative  to  Uic  direction 
of  its  movement  down  the  center  of  a  strip  of  diameter  d  can  be  written  as 

i  =  fi^(d/2)n/?±,/2(d/2). 


Note  that  while  L  may  consist  of  more  than  one  interval,  the  set  of  orientations  taken  by 
A,  in  some  trajectory  along  the  length  of  the  cone,  must  form  a  connected  set  -  i.e.  it  must 
be  a  single  interval,  and  so  must  be  contained  in  a  single  interval  of  JL 

Finally  note  that  the  sum  of  two  radius  functions  has  the  same  form  as  a  radius  function, 
and  so  the  “_L”  operation  can  be  computed  just  as  easily  on  the  sums  of  such  functions  as 
on  single  functions.  I.e.  with  R  as  above  and 

S(£)  =  max  e,  cos(£  —  /z.) 

1 

then 


[R  +  S](£)  =  R(i)  +  S{i)  =  max  (d<  cos(£  —  rj.)  +  tj  cos(£  —  /*,■)) 

i  <:*<>*,  i<cy<Cr72 

and  each  term  inside  the  “max”  can  be  written  in  the  form  fij  cos(£  — i/tJ). 

3.2  Bounding  polygons  with  an  appropriate  rectangle. 

In  general  we  wish  to  find  the  legal  orientations  for  object  A  when  its  origin  is  at  a 
point  with  spine  parameter  t  on  the  spine  of  a  generalized  cone  C.  A  conservative  set  of 
orientations  can  be  found  by  enclosing  A  in  a  rectangle  which  has  two  of  its  sides  parallel  to 
the  spine  of  C.  When  A  is  oriented  so  that  the  spine  has  angle  0  relative  to  it,  the  bounding 
rectangle  has  the  following  properties.  Its  extent  forward  (in  the  direction  of  increasing  spine 
parameter)  from  the  origin  is  R(0).  Its  extents  left  and  right  are  R(0  +  x/2)  and  R(0  —  7r/2) 
respectively.  Its  extent  to  the  rear  is  (rather  conservatively)  d  =  maxi<,<„dt.  Figure  8 
illustrates.  The  problem  now  is  to  invert  this  bounding  operation  -  i.e.  to  find  for  what 
range  of  0  the  bounding  rectangle  is  within  the  generalized  cone.  That  is  for  each  t  G  [0,1] 
we  wish  to  find  a  range  of  valid  0 ,  denoted  V(<)  for  orientations  of  the  object  A . 
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Figure  8.  An  object  A  is  bounded  by  a  rectangle  aligned  with  the  spine  of  cone  C. 

The  rear  of  the  bounding  rectangle  simply  implies  V(t)  =  0  outside  of  the  interval 
[<£,/].  The  forward  bound  implies 

V(t)  C  R^{1  - 1). 

The  parallel  sides  of  the  “big”  part  of  the  generalized  cone  imply  upper  bounds  on  the  width 
of  the  bounding  rectangle.  Thus 

V(t)  C  (R±(b,)  -  ir/2)  n  (R^(br)  +  t/2). 

Note  that  the  terms  on  the  right  involve  adding  a  constant  to  a  subset  of  [0,  2r\  in  the 
obvious  way.  Let  Vf  be  defined  as 

v\t)  =  r±(i  - t)n  -  */2)  n  (R^{br)  +  */2) 

over  the  interval  [d,/]. 

Subject  to  the  above  three  constraints  the  rectangle  can  everywhere  be  as  big  as  one 
which  will  fit  in  the  “small”  part  of  the  generalized  cone.  Thus 

((K-H*)  -  ir/2)  n  (/?-*-(*)  + 1/2))  n  V'[t)  c  n0- 
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Furthermore,  subject  to  V"(£),  any  orientation  of  A  is  valid  which  results  in  a  bounding 
rectangle  that  is  within  the  bounds  given  by  the  decreasing  boundaries  of  the  generalized 
cones.  Recall  that  at  each  point  of  the  spine,  with  parameter  f,  these  boundaries  have  both 
left  and  right  radius  of  mt  +  c  and  that  m  <  0.  Thus  sufficient  conditions  arc  that 

R[0  +  tt/2)  <  m  X  (t  +  R(0))  +  c 
R[0  -  7r/2)  <  m  X  (t  +  R{0))  +  c 

are  both  true.  These  can  be  expressed  as 

[R*/2  —  mR](0)  <  ( mt  +  c) 

—  mft](0)  <  [mt  4-  c) 

whence 


-  mR]±{mt  +  c)n  [R^/2  -  mR]±(mt  +  c))  n  V'(t)  C  V{t). 


In  summary  then,  over  the  range  t  £  [ d9l\ ,  a  valid  subset  of  orientations  for  A  can  be 
expressed  as 

V(0  =R^-(l  -t)D  (7l-f(6»)  -  ff/2)  n  (Rx(6r)  4-  ff/2) 
n  (((*-*-(«)  -  ir/2)  n  +  */2)) 

U  ([fi,/2  —  mR]XH  +  c)n  [R—,/2  —  mR^mt  +  c))) 
and  elsewhere  V(t)  =  0.  The  key  property  of  V(t)  is  the  following. 

lemma:  For  t\9  t<i  6  [d,Z],  t[  <  ^  implies  V^)  C  V(ti). 

Note  that  V(£)  does  not  necessarily  include  all  possible  orientations  for  object  A  to 
be  contained  in  cone  C  at  point  with  paremeter  t  along  the  spine.  However  it  the  set  of 
possibilities  which  the  find-path  algorithm  described  here  is  willing  to  consider.  Its  key 
property  is  that  as  stated  by  the  lemma  the  set  of  valid  orientations  (amongst  those  to  be 
considered)  does  not  increase  as  the  object  is  moved  from  the  "big”  end  to  the  "small”  end 
of  a  generalized  cone. 
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Figure  10.  A 


found  by  the  algorithm. 


4.  Searching  for  a  Path 


The  algorithm  to  find  a  collision  free  path  proceeds  as  follows.  First  the  generalized 
cones  which  describe  free  space  are  coomputcd  (this  can  be  done  in  lime  0(n*)).  Next  the 
cones  are  pairwise  examined  to  determine  if  and  where  their  spines  intersect.  Up  to  this 
stage  the  algorithm  is  independent  of  the  object  A  to  be  moved  through  the  environment. 
Each  spine  intersection  point  for  each  cone  is  now  annotated  with  the  subset  of  [0,2k] 
which  describes  the  orientations  for  the  object  A  in  which  it  is  guaranteed  to  be  completely 
contained  in  the  generalized  cone.  The  final  stage  of  the  algorithm  is  to  find  a  path  from  the 
initial  position  to  the  goal  position  following  the  spines  of  the  generalized  cones  and  changing 
from  cone  to  cone  at  spine  intersection  points.  If  the  object  is  kept  at  valid  orientations  at 
each  point  on  each  spine  then  the  path  will  be  collision  free. 

4.1  A  searchable  graph. 

A  graph  is  built  then  searched  with  the  A*  algorithm  (see  Nilsson  [7]).  The  cost  function 
used  is  the  distance  traveled  through  space. 

The  nodes  of  the  graph  consist  of  points  on  generalized  cone  spines  which  correspond 
to  points  of  intersection,  along  with  a  single  orientation  subinterval  of  [0, 2*]  (except  that 
intervals  may  wrap  around  from  2k  to  0).  Thus  for  an  intersection  point  with  parameter  t 
on  the  spine  of  cone  C,  a  node  for  each  interval  in  V{i)  is  built. 

The  arcs  in  the  graph  arise  in  two  ways;  those  that  correspond  to  transfer  from  one 
intersection  point  on  a  spine  to  another,  and  those  that  correspond  to  transfer  from  one 
spine  to  another  at  their  common  intersection  point.  All  such  candidate  pairs  of  nodes  are 
checked  for  connectivity.  The  lemma  of  section  3  guarantees  that  it  suffices  to  check  if  the 
orientation  intervals  of  the  two  nodes,  for  intra-cone  arcs,  have  a  non-empty  intersection. 
If  so,  then  it  is  certainly  possible  to  move  the  object  A  from  one  point  to  the  other  using 
any  orientation  within  that  intersection.  For  inter-cone  pairs  of  nodes  it  also  suffices  to 
check  for  non-empty  intersection  of  their  orientation  intervals,  as  the  points  are  coincident 
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t *  m 


in  apace. 


The  graph  can  now  be  searched  to  find  a  path  consisting  of  an  ordered  set  of  nodes 
from  initial  position  to  goal  position. 

4.2  Intermixing  rotations  and  translations. 

If  the  graph  search  is  successful  it  indicates  a  whole  class  of  collision  free  paths.  It 
remains  to  chose  one  particular  trajectory. 

» 

The  class  of  paths  found  share  a  common  trajectory  for  the  origin  of  the  moving  object. 
The  orientation  interval  associated  with  each  node  of  the  path  through  the  graph  constrains 
the  valid  orientations  at  that  point.  It  is  also  necessary  to  ensure  that  no  invalid  orientation 
is  assumed  during  translation  and  rotation  along  the  spine  of  a  generalized  cone. 

It  may  well  be  the  case  that  rotations  are  expensive,  and  it  is  worth  searching  the  class 
of  paths  resulting  from  the  graph  search  to  find  the  optimal  path  in  terms  of  least  rotation 
necessary.  This  could  be  carried  out  using  the  A*  algorithm.  Every  node  in  the  path  found 
in  the  first  search  would  be  turned  into  a  collection  of  nodes.  Each  pair  of  adjacent  nodes 
in  the  path  found  above  would  have  their  orientation  ranges  intersected.  Combine  each 
end  value  of  each  intersection,  and  the  initial  and  final  orientations  into  a  set  of  possible 
orientations.  Each  orientation  which  is  valid  for  a  particular  path  node,  along  with  that 
node,  generates  a  node  in  the  new  graph.  Adjacency  would  be  inherited  from  adjacency  of 
generating  nodes  in  the  original  path.  The  A*  agorithm  is  used  to  search  this  new  graph. 

Figures  9  and  10  illustrate  two  paths  found  by  a  much  simpler  method.  Rotations 
are  restricted  to  points  corresponding  to  nodes  of  the  path  -  i.e.  the  object  is  held  with 
fixed  orientation  during  each  translation.  The  orientation  at  each  point  is  the  midpoint  of 
the  orientations  given  by  intersecting  the  allowed  orientation  intervals  of  the  corresponding 
path  node,  and  the  subsequent  node.  Thus  traversal  of  intra-cone  arcs  of  the  graph  built 
above  are  rotation  free,  and  traversal  of  inter-cone  arcs  are  pure  rotations.  The  lemma  of 
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section  3  guarantees  that  this  strategy  leads  to  valid  collision  free  paths. 

5.  Usefulness  and  Applicability 

The  major  drawback  of  the  find-path  algorithm  presented  in  this  paper  is  that  paths 
arc  restricted  to  follow  the  spines  of  the  generalized  cones  chosen  to  represent  free  space.  It 
typically  does  not  work  well  in  tightly  constrained  spaces  as  there  arc  insufficient  generalized 
cones  to  provide  a  rich  choice  of  paths.  Furthermore,  in  a  tight  space  the  interior  points 
of  spines  are  always  near  to  the  spine  ends.  Therefore  the  need  to  extend  the  bounding 
rectangles  rearward  to  the  maximum  possible  distance  for  the  moving  object  (so  that  the 
lemma  of  section  3  will  hold)  means  that  typically  there  are  very  few  points  of  intersections 
of  generalized  cones  where  the  object’s  bounding  rectangle  in  each  orientation  is  contained 
within  the  appropriate  generalized  cone.  This  situation  could  be  improved  significantly  by 
developing  a  better  algorithm  for  decomposing  free  space  into  generalized  cones.  The  current 
pruning  technique  is  often  too  drastic  -  if  an  obstacle  intersects  a  generalized  cone  it  may 
be  better  to  simply  reduce  the  cone  radius,  rather  than  the  current  practice  of  slicing  out 
all  of  the  spine  onto  which  the  obstacle  projects. 

5.1  Advantages. 

In  relatively  uncluttered  environments  the  algorithm  is  extremely  fast.  Furthermore  it 
has  the  following  properties  which  are  absent  from  some  or  all  of  the  algorithms  mentioned 
in  section  1. 

1.  The  paths  found,  and  the  amount  of  computation  needed  are  completely  independent  of 
the  chosen  world  coordinate  system. 

2.  Obstacles  affect  the  representation  of  free  space  only  in  their  own  locality.  Thus  addi¬ 
tional  obstacles  spatially  separated  from  a  free  path  found  when  they  are  not  present  can 
not  affect  the  ability  to  find  that  free  path  then  they  are  present.  (Resource  limited,  and 
approximating  algorithms  which  cut  free  space  into  rectangloid  cells  often  suffer  from  this 
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problem.) 


3.  Free  space  is  represented  explicitly  rather  than  as  the  complement  of  forbidden  space. 

4.  The  paths  tend  to  be  equally  far  from  all  objects,  out  in  truly  free  space,  rather  than 
scraping  as  close  as  possible  to  obstacles,  making  the  instantiation  of  the  paths  by  mechanical 
devices  subject  to  failure  due  to  mechanical  imperfections. 

We  plan  to  use  this  algorithm  in  conjunction  with  an  implementation  of  an  algorithm 
([3])  which  finds  paths  with  rotations  based  on  a  configuration  space  approach.  The  general¬ 
ized  cone  method  will  be  us*d  U >  find  the  easy  parts  of  the  path  (the  other  method  is 
computationally  expensive  both  for  easy  and  hard  paths)  leaving  the  more  expensive  method 
for  the  hard  parts.  The  algorithm  presented  here  can  also  provide  some  direction  for  the 
hard  parts  as  it  can  quickly  compute  all  topologically  possible  paths. 

5.2  Three  Dimensions. 

Clearly  the  eventual  goal  of  this  work  is  to  develop  algorithms  which  solve  the  find- 
path  problem  in  three  dimensions  with  three  rotational  degrees  of  freedom  for  each  link  of 
general  articulated  devices.  In  this  section  we  discuss  approaches  to  extending  the  presented 
algorithm  to  the  case  of  a  single  convex  polyhedron  moving  through  a  space  of  polyhedral 
obstacles. 

Three  dimensional  generalized  cones  can  be  constructed  by  considering  all  triples  of 
faces  of  obstacle  polyhedra.  The  construction  algorithm  (including  intersection  with  obstacle 
polyhedra)  has  complexity  0(n4)  in  the  number  of  obstacle  faces.  The  generalized  cones 
so  constructed  will  have  triangular  cross-sections.  It  is  not  clear  how  to  pairwise  intersect 
the  generalized  cones,  as  in  general  their  spines  will  not  intersect.  The  "path-class’9  idea 
presented  below  may  solve  this  problem. 

Rather  than  a  bounding  rectangle,  a  right  triangular  prism  should  be  used  where  the 


triangle  is  similar  to  the  cross  section  of  the  generalized  cone  being  traversed.  If  more  than 
one  rotational  degree  of  freedom  is  assumed  there  may  be  problems  inverting  this  bounding 
volume.  Rather  than  V(t)  having  a  set  of  intervals  as  a  value,  it  will  have  a  set  of  three — 
dimensional  polyhedra. 

Both  for  the  two  dimensional  and  three  dimensional  problems  it  is  worth  investigating 
using  paths  other  than  the  spine  through  a  generalized  cone.  Then  V  is  parameterized  in 
terms  of  t  and  the  two  end-points  of  a  whole  class  of  paths  through  individual  cones.  This 
increases  the  complexity  of  the  search  but  will  lead  to  more  paths  than  simply  using  the 
spine. 

t 

5.3  Complexity  Issues. 

In  discussions  with  TomAs  Lozano-P6rez  it  became  evident  that  there  exists  an  0(n2cV^R  n) 
algorithm  which  can  find  the  same  generalized  cones  as  docs  the  0(n3)  algorithm  presented 
here.  The  spines  used  for  generalized  cones  correspond  to  the  Voronoi  boundaries  between 
obstacles  (see  Drysdalc  [4]  for  a  complete  solution  to  the  problem),  and  they  can  be  found  in 
0(nc V'1®*  n)  time.  There  remains  a  factor  of  O(n)  to  do  the  intersection  of  the  hypothesized 
generalized  cones  with  other  obstacles.  It  is  unlikely  that  such  an  algorithm  would  perform 
better  than  the  0(n3)  algorithm  presented  for  practical  sized  problems. 

The  complexity  of  pairwise  intersecting  the  generalized  cones  was  not  addressed  in  the 
body  of  the  paper.  An  upper  bound  of  0(nA)  can  easily  be  obtained  as  there  are  at  most 
0(n2)  generalized  cones  to  be  considered.  However,  it  seems  hard  to  fir  d  a  constant  where 
cn2  generalized  cones  can  be  acheived  constructively  as  the  number  of  obstacle  polygons  is 
increased.  For  any  c  it  seems  that  space  eventually  tends  to  become  too  cluttered  for  all 
the  cones  to  be  constructed.  Thus  it  seems  likely  that  a  better  bound  than  0(71*)  exists.  (In 
fact  the  Voronoi  complexity  above  suggests  a  bound  of  0(n2cVlo*n),) 
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